/**
 * @file Formation.h
 * @author GL (you@domain.com)
 * @brief 编队相关
 * @version 0.1
 * @date 2024-06-24
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#pragma once

#include <cmath>
#include <vector>
#include <random>

namespace formation{
    struct Point{
        double x,y,z;
        Point(){}
        Point(double px,double py,double pz):x(px),y(py),z(pz){}
    };

    enum FormationType {TRIANGLE=0,RECTANGLE=1,CIRCLE=2};
    void getFormationRelPose(std::vector<Point>& poses,double distance,FormationType type);  // 获取编队的相对位置
    void getTriangleFormationRelPose(std::vector<Point>& poses,double distance);
    void getRectFormationRelPose(std::vector<Point>& poses,double distance);
    void getCircleFormationRelPose(std::vector<Point>& poses,double distance);

    void generateCircleTraj(double omega,double R,std::vector<double>& T,std::vector<Point>& Velocity);  // 生成编队的圆形轨迹
    void generateRectTraj(double v,double L,std::vector<double>& T,std::vector<Point>& Velocity);  // 生成编队的矩形轨迹
    void generateRandomTraj(double v,std::vector<double>& T,std::vector<Point>& Velocity);  // 生成随机段轨迹
}